from machine import Pin, I2C, PWM
import math
led11 = Pin(16, Pin.OUT)
led12 = Pin(17, Pin.OUT)
led11.value(0)
led12.value(0)
def motor_run(a,b,c,d):
    motor1 = PWM(Pin(32))
    motor2 = PWM(Pin(26))
    motor3 = PWM(Pin(27))
    motor4 = PWM(Pin(13))
    led = Pin(33, Pin.OUT)
    led1 = Pin(25, Pin.OUT)
    led2 = Pin(14, Pin.OUT)
    led4 = Pin(12, Pin.OUT)
    led.value(1)
    led1.value(1)
    led2.value(1)
    led4.value(1)
    motor1.init(freq=50, duty=0)
    motor2.init(freq=50, duty=0)
    motor3.init(freq=50, duty=0)
    motor4.init(freq=50, duty=0)
    motor1.duty(int(1.023*a))
    motor2.duty(int(1.023*b))
    motor3.duty(int(1.023*c))
    motor4.duty(int(1.023*d))
    print(f"a:{a}")
Ki=0.005
Kp=1.5
halfT=0.005
s1=0;s2=0;s3=0
matrix=[]
clk = Pin(23, Pin.OUT)   # Select the 29 pin device as the clock
sda = Pin(22, Pin.IN)   # Select the 30 pin device as the data line
i2c = I2C(-1,clk, sda, freq=1000000) # create I2C peripheral at frequency of 100kH
j1=[];j2=[];j3=[];j4=[];j5=[];j6=[]
k=[]
NormAccz=0
RC_PIT=0.0
RC_ROL=0.0
RC_YAW=0.0
Butter_20HZ_Parameter_Acce = [
	[-0.00777631271910257, 0.06445464557871, 0.443321667140393],
	[0.443321667140393, 0.06445464557871, -0.00777631271910257]]
RC_THROTTLE = 0
YM_Dead=100
pid_out_Pitch=0 # 俯仰角外环pid
pid_out_Roll=0 # 横滚角外环pid
pid_in_Pitch=0 # 俯仰角内环pid
pid_in_Roll=0 # 横滚角内环pid
pid_in_Yaw=0 # 偏航角内环pid
MODE=0
Z_THRUST=0
pid1=0;pid2=0;pid3=0;pid4=0;pid5=0;pid6=0;pid7=0;pid8=0
PID_OUT_PITCH_KP=0.8
PID_OUT_PITCH_KI=0
PID_OUT_PITCH_KD=0.1
PID_OUT_ROLL_KP=0.8
PID_OUT_ROLL_KI=0
PID_OUT_ROLL_KD=0.1
PID_IN_PITCH_KP=1.15
PID_IN_PITCH_KI=0
PID_IN_PITCH_KD=1.2
PID_IN_ROLL_KP=1.15
PID_IN_ROLL_KI= 0
PID_IN_ROLL_KD=1.2
PID_IN_YAW_KP= 9
PID_IN_YAW_KI= 0.01
PID_IN_YAW_KD=0.5
PID_IN_Z_KP=1 
PID_IN_Z_KI= 0
PID_IN_Z_KD=1.5
PID_OUT_Z_KP=2 
PID_OUT_Z_KI= 0.05
PID_OUT_Z_KD=9
PID_IN_ACC_Z_KP=0.1 
PID_IN_ACC_Z_KI=0.001 
PID_IN_ACC_Z_KD=9
LPB_Cnt = 0
Buffer=[[0 for m in range(3)] for n in range(2)]
class PIDController:
    def __init__(self, Kp, Ki, Kd,desired,iLimit):
        self.Kp = Kp#比例项参数 Kp： 控制系统的灵敏度，如果设置太高，系统可能会过于敏感，引起振荡。如果设置太低，系统响应可能较慢。
        self.Ki = Ki#积分项参数 Ki： 控制系统的稳定性，用于消除系统静差。过高的积分项可能导致系统过度调整，产生超调。
        self.Kd =Kd #微分项参数 Kd： 控制系统的过冲和振荡，可以减缓系统的响应。然而，过高的微分项可能引入噪声。

        self.iLimit=iLimit
        self.deriv=0
        self.integ=0
        self.prevError=0
        self.error=0
        self.desired=desired

    def pidUpdate(self, measured,expect):
        output=0
        self.deriv = expect
        self.error = self.desired-measured
        self.integ += self.error
        if self.integ>self.iLimit:
            self.integ=self.iLimit
        else:
            if self.integ<self.iLimit:
                self.integ=-self.iLimit
        self.deriv=self.error-self.prevError
        self.outP=self.Kp*self.error
        self.outI=self.Ki *self.integ
        self.outD=self.Kd*self.deriv
        output=self.outD+self.outI+self.outP
        self.prevError=self.error
        return output
def PID_controllerInit():
    global pid1,pid2,pid3,pid4,pid5,pid6,pid7,pid8
    pid1=PIDController(Kp=PID_OUT_PITCH_KP, Ki=PID_OUT_PITCH_KI, Kd=PID_OUT_PITCH_KD,iLimit=500)#pid_out_Pitch
    pid2=PIDController(Kp=PID_OUT_ROLL_KP, Ki=PID_OUT_ROLL_KI, Kd=PID_OUT_ROLL_KD,iLimit=500)#pid_out_Roll
    pid3=PIDController(Kp=PID_IN_PITCH_KP, Ki=PID_IN_PITCH_KI, Kd=PID_IN_PITCH_KD,iLimit=500)#pid_in_Pitch
    pid4=PIDController(Kp=PID_IN_ROLL_KP, Ki=PID_IN_ROLL_KI, Kd=PID_IN_ROLL_KD,iLimit=500)#pid_in_Roll
    pid5=PIDController(Kp=PID_IN_YAW_KP, Ki=PID_IN_YAW_KI, Kd=PID_IN_YAW_KD,iLimit=300)#pid_in_Yaw
    pid6=PIDController(Kp=PID_IN_Z_KP, Ki=PID_IN_Z_KI, Kd=PID_IN_Z_KD,iLimit=200)#pid_in_Z
    pid7=PIDController(Kp=PID_OUT_Z_KP, Ki=PID_OUT_Z_KI, Kd=PID_OUT_Z_KD,iLimit=200)#pid_out_Z
    pid8=PIDController(Kp=PID_IN_ACC_Z_KP, Ki=PID_IN_ACC_Z_KI, Kd=PID_IN_ACC_Z_KD,iLimit=200)#pid_in_acc_Z
def LPButterworth(curr_input, Parameter):
    global LPB_Cnt,Buffer
    
    #Buffer[1]为Input_Butter
    #Buffer[0]为Output_Butter
    #Parameter[1]为b
    Buffer[1][2] = curr_input
    if (LPB_Cnt >= 100):
        Buffer[0][2] =Parameter[1][0] * Buffer[1][2] + Parameter[1][1] * Buffer[1][1] + Parameter[1][2] * Buffer[1][0] - Parameter[0][1] * Buffer[0][1] - Parameter[0][2] * Buffer[0][0]
    else:
        Buffer[0][2] = Buffer[1][2]
        LPB_Cnt+=1
    Buffer[1][0] = Buffer[1][1]
    Buffer[1][1] = Buffer[1][2]
    Buffer[0][0] = Buffer[0][1]
    Buffer[0][1] = Buffer[0][2]
    
    return  Buffer
def init_control():
    global pid_out_pitch,pid_out_roll,pid_in_Pitch,pid_in_Roll,pid_in_Yaw,RC_THROTTLE,k,MODE
    global Z_THRUST,u
    yaw_expect=0 
    pitch_expect=0
    roll_expect=0
    pitch_expect = RC_PIT / 40
    roll_expect = RC_ROL / 40
    yaw_expect = RC_YAW / 40
    MOTO1_PWM =0
    MOTO2_PWM =0 
    MOTO3_PWM =0 
    MOTO4_PWM=0
    u[1]=LPButterworth(s3,Butter_20HZ_Parameter_Acce)
    u[0]=LPButterworth(s2,Butter_20HZ_Parameter_Acce)
    if RC_THROTTLE > YM_Dead:
        pid_out_pitch = pid1.pidUpdate(u[1], pitch_expect)
        pid_out_roll = pid2.pidUpdate(u[0], roll_expect)
        pid_in_pitch = pid3.pidUpdate( (k[1] *57.324841 ), pid_out_pitch)
        pid_in_roll = pid4.pidUpdate(-(k[0] * 57.324841), pid_out_roll)
        pid_in_yaw = pid5.pidUpdate((k[2] * 57.324841), yaw_expect)
    else :
        if (RC_THROTTLE < YM_Dead): 
            pid_in_Pitchinteg = 0
            pid_in_Rollinteg = 0
            pid_in_Yawinteg = 0
    if (RC_THROTTLE > YM_Dead):
        if (MODE == 0):
            MOTO1_PWM = RC_THROTTLE + pid_in_pitch - pid_in_roll - pid_in_yaw
            MOTO2_PWM = RC_THROTTLE - pid_in_pitch - pid_in_roll + pid_in_yaw
            MOTO3_PWM = RC_THROTTLE - pid_in_pitch + pid_in_roll - pid_in_yaw
            MOTO4_PWM = RC_THROTTLE + pid_in_pitch + pid_in_roll + pid_in_yaw
        else:
            MOTO1_PWM = Z_THRUST * 1.2 + RC_THROTTLE + pid_in_pitch - pid_in_roll - pid_in_yaw
            MOTO2_PWM = Z_THRUST * 1.2 + RC_THROTTLE - pid_in_pitch - pid_in_roll + pid_in_yaw
            MOTO3_PWM = Z_THRUST * 1.2 + RC_THROTTLE - pid_in_pitch + pid_in_roll - pid_in_yaw
            MOTO4_PWM = Z_THRUST * 1.2 + RC_THROTTLE + pid_in_pitch + pid_in_roll + pid_in_yaw
    if (RC_THROTTLE > YM_Dead):
        if (MOTO1_PWM < 0):
            MOTO1_PWM = 0
        else :
            if (MOTO1_PWM > 900):
                MOTO1_PWM = 900
        if (MOTO2_PWM < 0):
            MOTO2_PWM = 0
        else :
            if (MOTO2_PWM > 900):
                MOTO2_PWM = 900
        if (MOTO3_PWM < 0):
            MOTO3_PWM = 0
        else :
            if (MOTO3_PWM > 900):
                MOTO3_PWM = 900
        if (MOTO4_PWM < 0):
            MOTO4_PWM = 0
        else :
            if (MOTO4_PWM > 900):
                MOTO4_PWM = 900
    else :
        MOTO1_PWM = 0;MOTO2_PWM = 0;MOTO3_PWM = 0 ;MOTO4_PWM = 0
    motor_run(int(MOTO1_PWM),int( MOTO2_PWM), int(MOTO3_PWM), int(MOTO4_PWM))
def init():
    i2c.writeto_mem(0x68, 0x6B, b'\x01')
    i2c.writeto_mem(0x68, 0x6C, b'\x00')
    i2c.writeto_mem(0x68, 0x19, b'\x05')
    i2c.writeto_mem(0x68, 0x1A, b'\x07')
    i2c.writeto_mem(0x68, 0x1B, b'\x18')
    i2c.writeto_mem(0x68, 0x1C, b'\x18')
init()
def zt(x,y):
    global Ki,Kp,halfT,matrix
    global s1,s2,s3
    global NormAccz,u
    q0=1;q1=0;q2=0;q3=0;exInt=0;eyInt=0;ezInt=0
    matrix.clear()
    ax=x[0];ay=x[1];az=x[2]
    gx=y[0];gy=y[1];gz=y[2]
    norm=math.sqrt(ax*ax+ay*ay+az*az)
    ax = ax * norm;ay = ay * norm;az = az * norm
    vx = 2 * ((q1*q3) - (q0*q2))
    vy = 2 * ((q0*q1) + (q2*q3))
    vz = (q0*q0) - (q1*q1) - (q2*q2) + (q3*q3)
    ex = ay * vz - az * vy
    ey = az * vx - ax * vz
    ez = ax * vy - ay * vx
    
    exInt = exInt + ex * Ki
    eyInt = eyInt + ey * Ki
    ezInt = ezInt + ez * Ki

    gx = gx + Kp * ex + exInt
    gy = gy + Kp * ey + eyInt
    gz = gz + Kp * ez + ezInt

    q0 = q0 + ((-q1) * gx - q2 * gy - q3 * gz) * halfT
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT
    
    norm=math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
    q0 = q0 * norm;q1 = q1 * norm;q2 = q2 * norm;q3 = q3 * norm
    print(f"ex:{q1}")
    matrix.append( (q0*q0) + (q1*q1) - (q2*q2) - (q3*q3))
    matrix.append(2 * ((q1*q2) + (q0*q3)))
    matrix.append(2* ((q1*q3) - (q0*q2)))   
    matrix.append ( 2 * ((q1*q2) - (q0*q3)))	   
    matrix.append ((q0*q0) - (q1*q1) + (q2*q2) - (q3*q3))
    matrix.append (2* ((q2*q3) + (q0*q1))	)   
    matrix.append(2* ((q1*q3) + (q0*q2)) ) 
    matrix.append ( 2* ((q2*q3) - (q0*q1)))   
    matrix.append  ((q0*q0) - (q1*q1) - (q2*q2) + (q3*q3))
    vecxZ = -2 *(q1*q3)+2*(q0*q2)
    vecyZ = 2* (q2*q3) + 2 * (q0*q1)	 
    veczZ = (q0*q0) - (q1*q1) - (q2*q2) + (q3*q3)
    s1=y[2]*0.0610351*0.002
    #s1=math.atan2(2* ((q1*q2) + (q0*q3)), -2*((q2*q2)-(q3*q3)+1))* 57.295779#yaw
    print(f"vecxZ:{vecxZ}")
    try:
        s3=math.asin(vecxZ) * 57.295779 #pitch
    except:
        if vecxZ<-1:
            s3=-3.14/2* 57.295779
        if vecxZ>1:
            s3=3.14/2* 57.295779
    s2=math.atan2(vecyZ, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.295779#roll
    if  s2>90 or s2<-90:
        if s3>0:
            s3=180-s3
        if s3<0:
            s3=-(180+s3)
    u=[s1,s2,s3]
    NormAccz=(x[0]*vecxZ+x[1]*vecyZ+x[2]*veczZ)
    return u
def pj(z):
    i=s=0
    z.sort()
    for i in range(0,10):
        s+=z[i]
    s=s/10
    return s
def pian(s):
    s=int(s.hex(),16)
    if s>0x8000:
        s=-(0xffff-s)
    return s
while True:
    i2c.scan()
    for i in range(0,10):
        x=i2c.readfrom_mem(0x68,0x43, 1)#GX
        x+=i2c.readfrom_mem(0x68,0x44, 1)
        y=i2c.readfrom_mem(0x68,0x45, 1)
        y+=i2c.readfrom_mem(0x68,0x46, 1)
        z=i2c.readfrom_mem(0x68,0x47, 1)
        z+=i2c.readfrom_mem(0x68,0x48, 1)

        x1=i2c.readfrom_mem(0x68,0x3B, 1)#ax
        x1+=i2c.readfrom_mem(0x68,0x3C, 1)
        y1=i2c.readfrom_mem(0x68,0x3D, 1)
        y1+=i2c.readfrom_mem(0x68,0x3E, 1)
        z1=i2c.readfrom_mem(0x68,0x3F, 1)
        z1+=i2c.readfrom_mem(0x68,0x40, 1)
       
        z2=pian(z1)
        x2=pian(x1)
        y2=pian(y1)
        z22=pian(z)
        x22=pian(x)
        y22=pian(y)

        j1.append(z22)
        j2.append(x22)
        j3.append(y22)
        j4.append(z2)
        j5.append(x2)
        j6.append(y2)  
    x11=pj(j5)/32768*16*9.8
    print(x11)
    y11=pj(j6)/32768*16*9.8
    print(y11)
    z11=pj(j4)/32768*16*9.8
    print(z11)
    l=[x11,y11,z11]
    x12=pj(j2)/16.4*3.1415/180
    print(x12)
    y12=pj(j3)/16.4*3.1415/180
    print(y12)
    z12=pj(j1)/16.4*3.1415/180
    print(z12)
    k=[x12,y12,z12]
    print(k)
    print(l)
    print(f"zt:{zt(l,k)}")
    j1.clear()
    j2.clear()
    j3.clear()
    j4.clear()
    j5.clear()
    j6.clear()
    init_control()
    
